package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.debugger.Log;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import org.nashua.tt151.dashboard.Dashboard;

public class Drive {

    public static CANJaguar createJag(int slot){
        //creates a CANJag with ID slot
        try{
            return new CANJaguar(slot);
        }catch(Exception e){
            System.out.println("CANJag "+slot+" shat the bed while initializing");
            Log.log("CANJag "+slot+" shat the bed while initializing");
            return null;
        }
    }
    public static void setJag(CANJaguar jag,double set){
        //sets double to CANJag
        try{
            jag.setX(set);
        }catch(Exception e){
            System.out.println("CANJag "+jag+" shat the bed while setting");
            Log.log("CANJag "+jag+" shat the bed while setting");
        }
    }
    public static double readJag(CANJaguar jag){
        //returns double from read CANJag
        try{
            return jag.getX();
        }catch(Exception e){
            System.out.println("CANJag "+jag+" shat the bed while reading");
            Log.log("CANJag "+jag+" shat the bed while reading");
            return 0;
        }
    }
    public static void setJagControl(CANJaguar jag){
        //sets jaguar to voltage control type
        try{
            jag.changeControlMode(CANJaguar.ControlMode.kVoltage);
        }catch(Exception e){
            System.out.println("CANJag "+jag+" shat the bed while being converted to voltage control");
            Log.log("CANJag "+jag+" shat the bed while being converted to voltage control");
        }
    }
    public static void manualDrive(Joystick joy,CANJaguar fl,CANJaguar fr,CANJaguar bl,CANJaguar br,double power,boolean inverse){
        //manual control for the wheel drive systems
        //flipping front and back or drive is determined by variable 'inverse'
        try{
            setJag(fl,-joy.getRawAxis(inverse?RobotMap.DUALACTION_RIGHT_Y:RobotMap.DUALACTION_LEFT_Y)*power*(inverse?-1:1));
            setJag(fr,joy.getRawAxis(inverse?RobotMap.DUALACTION_LEFT_Y:RobotMap.DUALACTION_RIGHT_Y)*power*(inverse?-1:1));
            setJag(bl,readJag(fl));
            setJag(br,readJag(fr));
        }catch(Exception e){
            System.out.println("Drive CANJags shat the bed while driving");
            Log.log("Drive CANJags shat the bed while driving");
        }
    }
    public static void manualPickup(Joystick joy,int joySlot,CANJaguar jag,boolean[][][] bools){
        //manual control of the pickup system
        setJag(jag,Drive.toggleTest(joy,joySlot,RobotMap.DUALACTION_LEFT_BUMPER,bools)?-.5:0);
        setJag(jag,Drive.toggleTest(joy,joySlot,RobotMap.DUALACTION_RIGHT_BUMPER,bools)?.5:0);
    }
    public static void manualDrop(Joystick joy,Servo s){
        //drops servo while 'A' is held down, starts returning as soon as 'A'  is released
        s.set(!joy.getRawButton(RobotMap.DUALACTION_BUTTON_A)?RobotMap.V_SERVO_DROPPER_DOWN:RobotMap.V_SERVO_DROPPER_DEF);
    }
    public static void manualShooter(double voltage,Joystick joy,Servo yaw,Servo pitch,Jaguar topJag,Jaguar lowJag){
        //double power = autoAim(pitch,yaw,hoop,led,dash);
        //manual control of the shooter's functions
        shooterPitchYaw(joy,pitch,yaw);//allows pitch/yaw control from independant joysticks
        shooterPower(voltage,topJag,lowJag);//sets voltage to shooter motors
        
    }
    private static void shooterPitchYaw(Joystick shooter,Servo pitch, Servo yaw) {
        double y=shooter.getRawAxis(RobotMap.DUALACTION_RIGHT_Y);
        double x=shooter.getRawAxis(RobotMap.DUALACTION_LEFT_X);
        //if(shooter.getRawAxis(RobotMap.DUALACTION_RIGHT_Y)>.5&&pitchPos<RobotMap.V_SERVO_PITCH_UP)pitchPos+=0.1;
        //if(shooter.getRawAxis(RobotMap.DUALACTION_RIGHT_Y)<-.5&&pitchPos>RobotMap.V_SERVO_PITCH_DEF)pitchPos-=0.1
        pitch.set(scaleValue(y<0?-y:RobotMap.V_SERVO_PITCH_DEF,RobotMap.V_SERVO_PITCH_DOWN,RobotMap.V_SERVO_PITCH_UP));
        yaw.set(scaleValue(x<0?-x:RobotMap.V_SERVO_YAW_DEF,RobotMap.V_SERVO_YAW_LEFT,RobotMap.V_SERVO_YAW_RIGHT));
    }
    private static void shooterPower(double voltage,Jaguar topJag,Jaguar lowJag) {
        topJag.setRaw((int)(128-voltage*(127/12)));
        lowJag.setRaw((int)(128-voltage*(127/12)));
    }
    public static double autoAim(Servo pitch, Servo yaw, int hoop, DigitalOutput led, Dashboard dash) {
        try {
            led.set(true);
            String sho = dash.requestInfo("SHOOT-T"+hoop);
            double pm = 180;
            double ym = 1260;
            pitch.set(scaleValue(pitch.get()+Double.parseDouble(sho.substring(0,sho.indexOf(",")))/pm,RobotMap.V_SERVO_PITCH_DOWN,RobotMap.V_SERVO_PITCH_UP));
            yaw.set(scaleValue(yaw.get()+Double.parseDouble(sho.substring(sho.indexOf(",")+1,sho.indexOf("~")))/ym,RobotMap.V_SERVO_YAW_LEFT,RobotMap.V_SERVO_YAW_RIGHT));
            return Double.parseDouble(sho.substring(sho.indexOf("~")))/12.0;
        } catch (Exception e){
            Log.log("WARNING: Failed To AutoAim - "+e.getMessage());
            return 0;
        }
    }
    private static double scaleValue(double orig, double min, double max){
        //scales a value to be between two given values
        return orig*(max-min)+min;
    }
    public static boolean toggleTest(Joystick j,int p,int b,boolean[][][] bool){
        //boolean toggle test mapping for all buttons on both joysticks
        if(bool[p-1][b-1][0]){
            if(!j.getRawButton(b)){
                bool[p-1][b-1][0]=false;
                bool[p-1][b-1][1]=!bool[p-1][b-1][1];
            }
        }else{
            if(j.getRawButton(b))bool[p-1][b-1][0]=true;
        }
        return bool[p-1][b-1][1];
    }
}